using System;
using MetaViewAnisotropicDiffusion.Matrix;
using System.Runtime.InteropServices;
using System.Drawing;
using System.Drawing.Drawing2D;
using System.Collections;
using System.IO;

namespace MetaViewAnisotropicDiffusion
{
	namespace PeronaMalik
	{
		public class Tools
		{

			public static ComplexDoubleMatrix Gradient(DoubleMatrix image)
			{
				ComplexDoubleMatrix tmp = new ComplexDoubleMatrix(image.XDim, image.YDim);
				// Calculate dx
				for (int j=0; j<image.YDim; j++) 
				{
					tmp.Real.data[0,j] = image.data[1,j] - image.data[0,j];
					tmp.Real.data[image.XDim-1,j] = image.data[image.XDim-1,j] - image.data[image.XDim-2,j];
				}
				for (int i=1; i<image.XDim-1; i++) 
				{
					for (int j=0; j<image.YDim; j++) 
					{
						tmp.Real.data[i,j] = image.data[i+1,j] - image.data[i-1,j];
					}
				}

				// Calculate dy
				for (int i=0; i<image.XDim; i++) 
				{
					tmp.Imag.data[i,0] = image.data[i,1] - image.data[i,0];
					tmp.Imag.data[i,image.YDim-1] = image.data[i, image.YDim-1] - image.data[i, image.YDim-2];
				}
				for (int i=0; i<image.XDim; i++) 
				{
					for (int j=1; j<image.YDim-1; j++) 
					{
						tmp.Imag.data[i,j] = image.data[i,j+1] - image.data[i,j-1];
					}
				}

				tmp.real.DivideValue(2d);
				tmp.imag.DivideValue(2d);

				return tmp;
			}

			public static ComplexDoubleMatrix[] Gradient(MultiValuedDoubleMatrix image)
			{
				ComplexDoubleMatrix[] tmp = new ComplexDoubleMatrix[image.VectorLength];
				for(int i=0;i<image.VectorLength;i++)
				{
					tmp[i] = Gradient(image.comp[i]);
				}
				return tmp;
			}


			public static ComplexDoubleMatrix GradientMulti(MultiValuedDoubleMatrix image)
			{
				ComplexDoubleMatrix tmp = new ComplexDoubleMatrix(image.XDim,image.YDim,0d);
				ComplexDoubleMatrix[] grads = new ComplexDoubleMatrix[image.VectorLength];
				for(int i=0;i<image.VectorLength;i++)
				{
					grads[i] = Tools.Gradient(image.comp[i]);
				}
				

				// 0: dxdy 1: dxdx 2: dydy
				DoubleMatrix[] tensor = new DoubleMatrix[3];
				tensor[0] = new DoubleMatrix(image.XDim, image.YDim, 0.0);
				tensor[1] = new DoubleMatrix(image.XDim, image.YDim, 0.0);
				tensor[2] = new DoubleMatrix(image.XDim, image.YDim, 0.0);
				
				for (int i=0; i<image.XDim; i++) 
				{
					for (int j=0; j<image.YDim; j++) 
					{
						for(int z=0;z<image.VectorLength;z++)
						{
							tensor[0].data[i,j] += grads[z].Real.data[i,j]*grads[z].Imag.data[i,j];
							tensor[1].data[i,j] += grads[z].Real.data[i,j]*grads[z].Real.data[i,j];
							tensor[2].data[i,j] += grads[z].Imag.data[i,j]*grads[z].Imag.data[i,j];
						}


						double a = tensor[1].data[i,j];
						double b = tensor[0].data[i,j];
						double c = tensor[2].data[i,j];

						double eig = ((a+c)+Math.Sqrt((a-c)*(a-c)+4*b*b))/2;
						double theta = Math.Atan2(eig-a,b);
						tmp.real.data[i,j] = Math.Sqrt(eig)*Math.Cos(theta);
						tmp.imag.data[i,j] = Math.Sqrt(eig)*Math.Sin(theta);
					}
				}

				return tmp;
			}


			// Adapted from David Royal Martin's Matlab code:
			public static DoubleMatrix NonMaximaSuppress(DoubleMatrix edgesMain, ComplexDoubleMatrix vectors)
			{
				DoubleMatrix edges = edgesMain.Clone();

				edges.UnitScale();


				IntMatrix mask = NonMaximaMask(edges, vectors);

				for(int i=0;i<edges.XDim;i++)
				{
					for(int j=0;j<edges.YDim;j++)
					{
						if(mask.data[i,j] == 0)
						{
							edgesMain.data[i,j] = 0d;
						}

					}
				}


				return edgesMain;

			}

			public static DoubleMatrix NonMaximaSuppress(DoubleMatrix edges, ComplexDoubleMatrix vectors1, ComplexDoubleMatrix vectors2)
			{
				edges.UnitScale();


				IntMatrix mask1 = NonMaximaMask(edges, vectors1);
				IntMatrix mask2 = NonMaximaMask(edges, vectors2);

				for(int i=0;i<edges.XDim;i++)
				{
					for(int j=0;j<edges.YDim;j++)
					{
						if(mask1.data[i,j] == 0 && mask2.data[i,j] == 0 )
						{
							edges.data[i,j] = 0d;
						}
					}
				}


				return edges;

			}


			public static IntMatrix NonMaximaMask(DoubleMatrix edges, ComplexDoubleMatrix vectors)
			{
				IntMatrix mask = new IntMatrix(edges.XDim, edges.YDim, 1);
				DoubleMatrix theta = new DoubleMatrix(edges.XDim, edges.YDim, 0d);
				
				IntMatrix mask15 = new IntMatrix(edges.XDim, edges.YDim, 0);
				IntMatrix mask26 = new IntMatrix(edges.XDim, edges.YDim, 0);
				IntMatrix mask37 = new IntMatrix(edges.XDim, edges.YDim, 0);
				IntMatrix mask48 = new IntMatrix(edges.XDim, edges.YDim, 0);

				for(int i=0;i<theta.XDim;i++)
				{
					for(int j=0;j<theta.YDim;j++)
					{

						theta.data[i,j] = Tools.Direction(vectors.imag.data[i,j], vectors.real.data[i,j]);

						if( theta.data[i,j] >= 0d && theta.data[i,j] < Math.PI/4d )
						{
							mask15.data[i,j] = 1;
						}

						else if( theta.data[i,j] >= Math.PI/4d && theta.data[i,j] < Math.PI/2d )
						{
							mask26.data[i,j] = 1;
						}

						else if( theta.data[i,j] >= Math.PI/2d && theta.data[i,j] < Math.PI*3d/4d )
						{
							mask37.data[i,j] = 1;
						}

						else if( theta.data[i,j] >= Math.PI*3d/4d && theta.data[i,j] < Math.PI )
						{
							mask48.data[i,j] = 1;
						}

					}

				}



				//Case 1
				for(int i=0;i<theta.XDim-1;i++)
				{
					for(int j=0;j<theta.YDim-1;j++)
					{
						if(mask15.data[i,j] == 1)
						{
							double d = Math.Tan(theta.data[i,j]);
							double interpolated = edges.data[i+1,j]*(1-d) + edges.data[i+1,j+1]*d;
							if(edges.data[i,j]<interpolated)
							{
								mask.data[i,j] = 0;
							}
						}
					}
				}

				//Case 5
				for(int i=1;i<theta.XDim;i++)
				{
					for(int j=1;j<theta.YDim;j++)
					{
						if(mask15.data[i,j] == 1)
						{
							double d = Math.Tan(theta.data[i,j]);
							double interpolated = edges.data[i-1,j]*(1-d) + edges.data[i-1,j-1]*d;
							if(edges.data[i,j]<interpolated)
							{
								mask.data[i,j] = 0;
							}
						}
					}
				}


				//Case 2
				for(int i=0;i<theta.XDim-1;i++)
				{
					for(int j=0;j<theta.YDim-1;j++)
					{
						if(mask26.data[i,j] == 1)
						{
							double d = Math.Tan(Math.PI/2d - theta.data[i,j]);
							double interpolated = edges.data[i,j+1]*(1-d) + edges.data[i+1,j+1]*d;
							if(edges.data[i,j]<interpolated)
							{
								mask.data[i,j] = 0;
							}
						}
					}
				}

				//Case 6
				for(int i=1;i<theta.XDim;i++)
				{
					for(int j=1;j<theta.YDim;j++)
					{
						if(mask26.data[i,j] == 1)
						{
							double d = Math.Tan(Math.PI/2d - theta.data[i,j]);
							double interpolated = edges.data[i,j-1]*(1-d) + edges.data[i-1,j-1]*d;
							if(edges.data[i,j]<interpolated)
							{
								mask.data[i,j] = 0;
							}
						}
					}
				}




				//Case 3
				for(int i=1;i<theta.XDim;i++)
				{
					for(int j=0;j<theta.YDim-1;j++)
					{
						if(mask37.data[i,j] == 1)
						{
							double d = Math.Tan(theta.data[i,j] - Math.PI/2d);
							double interpolated = edges.data[i,j+1]*(1-d) + edges.data[i-1,j+1]*d;
							if(edges.data[i,j]<interpolated)
							{
								mask.data[i,j] = 0;
							}
						}
					}
				}


				//Case 7
				for(int i=0;i<theta.XDim-1;i++)
				{
					for(int j=1;j<theta.YDim;j++)
					{
						if(mask37.data[i,j] == 1)
						{
							double d = Math.Tan(theta.data[i,j] - Math.PI/2d);
							double interpolated = edges.data[i,j-1]*(1-d) + edges.data[i+1,j-1]*d;
							if(edges.data[i,j]<interpolated)
							{
								mask.data[i,j] = 0;
							}
						}
					}
				}


				//Case 4
				for(int i=1;i<theta.XDim;i++)
				{
					for(int j=0;j<theta.YDim-1;j++)
					{
						if(mask48.data[i,j] == 1)
						{
							double d = Math.Tan(Math.PI - theta.data[i,j]);
							double interpolated = edges.data[i-1,j]*(1-d) + edges.data[i-1,j+1]*d;
							if(edges.data[i,j]<interpolated)
							{
								mask.data[i,j] = 0;
							}
						}
					}
				}


				//Case 8
				for(int i=0;i<theta.XDim-1;i++)
				{
					for(int j=1;j<theta.YDim;j++)
					{
						if(mask48.data[i,j] == 1)
						{
							double d = Math.Tan(Math.PI - theta.data[i,j]);
							double interpolated = edges.data[i+1,j]*(1-d) + edges.data[i+1,j-1]*d;
							if(edges.data[i,j]<interpolated)
							{
								mask.data[i,j] = 0;
							}
						}
					}
				}

				return mask;

			}


			public static double Direction(double y, double x)
			{
				if(x>=0 && y>=0)
				{
					return Math.Atan2(y,x);
				}
				else if(x<0 && y == 0d)
				{
					return Math.Atan2(y,x) - Math.PI;
				}
				else if(x<0 && y>0)
				{
					return Math.Atan2(y,x);
				}
				else if(x<=0 && y<0)
				{
					return Math.PI + Math.Atan2(y,x) ;
				}
				else if(x>0 && y<0)
				{
					return Math.PI + Math.Atan2(y,x) ;
				}
				else 
				{
					Console.WriteLine("Something went wrong with Direction(x,y) function.. Returning -1..\n");
					return -1.0;
				}

			}





			public class CompareGrads : IComparer  
			{
				public static DoubleMatrix gradMatrix;
				int IComparer.Compare( Object x, Object y )  
				{
					Point px = (Point)x;
					Point py = (Point)y;
					if(gradMatrix.data[px.X,px.Y] > gradMatrix.data[py.X,py.Y])
					{
						return 1;
					}
					else if(gradMatrix.data[px.X,px.Y] == gradMatrix.data[py.X,py.Y])
					{
						return 0;
					}
					else
					{
						return -1;
					}
				}

			}




			public static IntMatrix GetEgdes(DoubleMatrix grads, IntMatrix thick, IntMatrix suppressed)
			{
				IntMatrix edges = new IntMatrix(grads.XDim, grads.YDim, 0);

				int[] LabelCount = new int[grads.XDim*grads.YDim];
				for(int i=0;i<grads.XDim*grads.YDim;i++)
				{
					LabelCount[i] = 0;
				}

				double[] LabelAvg = new double[grads.XDim*grads.YDim];
				for(int i=0;i<grads.XDim*grads.YDim;i++)
				{
					LabelAvg[i] = 0d;
				}

				
				IntMatrix labels = new IntMatrix(grads.XDim, grads.YDim, 0);

				IntMatrix dummy = new IntMatrix(grads.XDim, grads.YDim, 0);
				int label = 0;

				Queue outsQ = new Queue();

				Queue tmpQ = new Queue();

				for (int x=0; x<grads.XDim; x++) 
				{
					for (int y=0; y<grads.YDim; y++) 
					{
						if(dummy.data[x,y] == 0 && thick.data[x,y] == 0)
						{
							label++;
							tmpQ.Enqueue(new Point(x,y));
							dummy.data[x,y] = 1;
							labels.data[x,y] = label;
							LabelCount[label]++;
							LabelAvg[label] += grads.data[x,y];

							while(tmpQ.Count > 0)
							{
								Point p = (Point)tmpQ.Dequeue();

								for (int i=-1; i<=1; i++) 
								{
									for (int j=-1; j<=1; j++) 
									{
										if(i == 0 || j == 0) 
										{
											if(p.X+i>=0 && p.X+i<grads.XDim && p.Y+j>=0 && p.Y+j<grads.YDim) 
											{
												if(dummy.data[p.X+i,p.Y+j] == 0 && thick.data[p.X+i,p.Y+j] == 0)
												{
													tmpQ.Enqueue(new Point(p.X+i,p.Y+j));
													dummy.data[p.X+i,p.Y+j] = 1;
													labels.data[p.X+i,p.Y+j] = label;
													LabelCount[label]++;
													LabelAvg[label] += grads.data[p.X+i,p.Y+j];
												}
											}
										}
									}
								}
							}

						}
						else if(thick.data[x,y] == 1)
						{
							outsQ.Enqueue(new Point(x,y));
						}
					}
				}


				for(int i=1;i<=label;i++)
				{
					LabelAvg[i] /= LabelCount[i];
				}
				



				// Utilize suppressed edges and use sorting for gradients...
				// Region growing here...

				CompareGrads.gradMatrix = grads;

				IntMatrix tmp = new IntMatrix(grads.XDim, grads.YDim,0);
				Hashtable Marked = new Hashtable();
				bool greedy = false;
				int counter = 0;
				while(outsQ.Count > 0)
				{
					counter++;
					Marked.Clear();
					int len = outsQ.Count;
					//Console.WriteLine("Length: {0}", len);

					ArrayList psA = new ArrayList(outsQ);
					outsQ.Clear();
					IComparer myComparer = new CompareGrads();
					psA.Sort(myComparer);

					int marked = 0;
					foreach(Point p in psA)
					{
						if(labels.data[p.X,p.Y] == 0)
						{
							ArrayList A = new ArrayList();


							for (int i=-1; i<=1; i++) 
							{
								for (int j=-1; j<=1; j++) 
								{
									if(i == 0 || j == 0) 
									{
										if(p.X+i>=0 && p.X+i<grads.XDim && p.Y+j>=0 && p.Y+j<grads.YDim) 
										{
											if(labels.data[p.X+i,p.Y+j] != 0)
											{
												A.Add(new Point(p.X+i,p.Y+j));
											}
										}
									}
								}
							}

							if(A.Count == 0)
							{
								outsQ.Enqueue(p);
							}
							else if(A.Count == 1)
							{
								Point p2 = (Point)A[0];
								if(suppressed.data[p.X,p.Y] == 0 || greedy)
								{
									labels.data[p.X,p.Y] = labels.data[p2.X,p2.Y];
									marked++;
								}
								else
								{
									outsQ.Enqueue(p);
								}
							}
							else
							{
								Point pp = (Point)A[0];
								bool equal = true;
								foreach(Point pp2 in A)
								{
									if(labels.data[pp.X,pp.Y] != labels.data[pp2.X,pp2.Y])
									{
										equal = false;
									}
								}

								if(equal)
								{
									if(suppressed.data[p.X,p.Y] == 0 || greedy)
									{
										labels.data[p.X,p.Y] = labels.data[pp.X,pp.Y];
										marked++;
									}
									else
									{
										outsQ.Enqueue(p);
									}
								}
							}
						}
						else
						{
							Console.WriteLine("Something is wrong: A labeled point is added to the Queue!!!");
						}
					}


					if(marked == 0 && greedy)
					{
						break;
					}

					if(marked == 0)
					{
						greedy = true;
					}
				}
				






				for (int x=0; x<grads.XDim; x++) 
				{
					for (int y=0; y<grads.YDim; y++) 
					{
						if(labels.data[x,y] == 0)
						{
							edges.data[x,y] = 1;
						}
					}
				}

				//Console.WriteLine("END");

				return edges;


			}











		}
	}
}




